#!/usr/bin/python3 
# -*- coding: utf-8 -*-
import time
import datetime
import platform 
import serial
global SerialHdl

#////////////////  宏定义 勿改动 //////////////////
DEMO_MODE_CYCLE = 0  #demo 循环播放 
DEMO_MODE_ONCE  = 1  #demo 只播放一次
ABSA            = 0  # 动作描述中角度信息的类型: 绝对值 (无视舵机当前角度，直接指定舵机角度)
RELA            = 1  # 动作描述中角度信息的类型: 相对值 (以舵机当前角度为基准偏移指定角度)
SERVO_SUM_MAX   = 9  # 参与动作的舵机数量
#////////////////  功能配置宏定义 充分理解含义后再改动 //////////////////
SERVO_SUM       = 9
DEMO_MODE       = DEMO_MODE_ONCE

#舵机群基础动作库 
servo_motion_lib = [
#servoID:    1#               2#               3#               4#               5#               6#               7#               8#               9#   
#--------------------------------------------------------------------------------------------------------------------------------------------------------------------------
#0#动作: 1#舵机角度归零,耗时1秒 其他不变    
      [[  0,ABSA, 1000],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0], ],
#1#动作: 2#舵机角度归零,耗时1秒 其他不变       
      [[  0,RELA,    0],[  0,ABSA, 1000],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0], ],
#2#
      [[  0,RELA,    0],[  0,RELA,    0],[  0,ABSA, 1000],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0], ],
#3#
      [[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,ABSA, 1000],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0], ],
#4#
      [[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,ABSA, 1000],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0], ],
#5#
      [[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,ABSA, 1000],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0], ],
#6#
      [[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,ABSA, 1000],[  0,RELA,    0],[  0,RELA,    0], ],
#7#
      [[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,ABSA, 1000],[  0,RELA,    0], ],
#8#
      [[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,ABSA, 1000], ],
#9#动作: 9#舵机顺时针+90度 耗时1秒 其他不变
      [[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[ 90,RELA, 1000], ],
#10#动作: 8#舵机顺时针+90度 耗时1秒 其他不变
      [[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[ 90,RELA, 1000],[  0,RELA,    0], ],
#11#
      [[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[ 90,RELA, 1000],[  0,RELA,    0],[  0,RELA,    0], ],
#12#
      [[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[ 90,RELA, 1000],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0], ],
#13#
      [[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[ 90,RELA, 1000],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0], ],
#14#
      [[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[ 90,RELA, 1000],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0], ],
#15#
      [[  0,RELA,    0],[  0,RELA,    0],[ 90,RELA, 1000],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0], ],
#16#
      [[  0,RELA,    0],[ 90,RELA, 1000],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0], ],
#17#
      [[ 90,RELA, 1000],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0], ],
#18# 所有舵机顺时针+90度 耗时1秒
      [[ 90,RELA, 1000],[ 90,RELA, 1000],[ 90,RELA, 1000],[ 90,RELA, 1000],[ 90,RELA, 1000],[ 90,RELA, 1000],[ 90,RELA, 1000],[ 90,RELA, 1000],[ 90,RELA, 1000], ],
#19# 所有舵机逆时针-90度 耗时1秒
      [[-90,RELA, 1000],[-90,RELA, 1000],[-90,RELA, 1000],[-90,RELA, 1000],[-90,RELA, 1000],[-90,RELA, 1000],[-90,RELA, 1000],[-90,RELA, 1000],[-90,RELA, 1000], ],
#20#动作: 1#舵机逆时针-90度 其他不变
      [[-90,RELA, 1000],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0], ],
#21#动作: 2#舵机逆时针-90度 其他不变
      [[  0,RELA,    0],[-90,RELA, 1000],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0], ],
#22# 
      [[  0,RELA,    0],[  0,RELA,    0],[-90,RELA, 1000],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0], ],
#23# 
      [[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[-90,RELA, 1000],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0], ],
#24# 
      [[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[-90,RELA, 1000],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0], ],
#25# 
      [[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[-90,RELA, 1000],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0], ],
#26# 
      [[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[-90,RELA, 1000],[  0,RELA,    0],[  0,RELA,    0], ],
#27# 
      [[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[-90,RELA, 1000],[  0,RELA,    0], ],
#28# 
      [[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[-90,RELA, 1000], ],
#29#动作: 1#舵机角度180,耗时1秒 其他不变    
      [[180,ABSA, 1000],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0], ],
#30#动作: 2#舵机角度180,耗时1秒 其他不变       
      [[  0,RELA,    0],[180,ABSA, 1000],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0], ],
#31#
      [[  0,RELA,    0],[  0,RELA,    0],[180,ABSA, 1000],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0], ],
#32#
      [[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[180,ABSA, 1000],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0], ],
#33#
      [[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[180,ABSA, 1000],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0], ],
#34#
      [[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[180,ABSA, 1000],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0], ],
#35#
      [[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[180,ABSA, 1000],[  0,RELA,    0],[  0,RELA,    0], ],
#36#
      [[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[180,ABSA, 1000],[  0,RELA,    0], ],
#37#
      [[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[  0,RELA,    0],[180,ABSA, 1000], ],
	]

#舵机群动作序列 (存储在FLASH中)
#将各种基础动作通过不同的延时粘合在一起，构成复杂的动作序列
servo_demo = [
	#///////////////////////////   位置初始化 全部躺下   //////////////////////////////////////
	#//全部角度归零(横躺) 标准速度
	[ 0,    0,100], [ 1,    0,100], [ 2,    0,100], [ 3,    0,100], [ 4,    0,100], [ 5,    0,100], [ 6,    0,100], [ 7,    0,100], [ 8, 2000,100], 
	#/////////////////////////////////////////////////////////////////
	#//反序挨个竖起 110% speed
	[ 9,  900, 90], [10,  900, 90], [11,  900, 90], [12,  900, 90], [13,  900, 90], [14,  900, 90], [15,  900, 90], [16,  900, 90], [17, 1500, 90], 
	# #//顺时针全部躺下 120% 
	[18, 1500, 80],
	# #//正序挨个竖起 130%
	[20,  700, 70], [21,  700, 70], [22,  700, 70], [23,  700, 70], [24,  700, 70], [25,  700, 70], [26,  700, 70], [27,  700, 70], [28, 1500,70], 
	# #/////////////////////////////////////////////////////////////////
	# #//逆时针正序挨个躺下 但不等前一个完全躺下 下一个就要开始动作 标准速度
	[20,  130, 60], [21,  130, 60], [22,  130, 60], [23,  130, 60], [24,  130, 60], [25,  130, 60], [26,  130, 60], [27,  130, 60], [28, 1500, 60], 
	# #//正序 奇数舵机先竖起 反序偶数舵机后竖起
	[17,  800, 80], [15,  800, 80], [13,  800, 80], [11,  800, 80], [ 9,  800, 80], 
	[10,  800, 80], [12,  800, 80], [14,  800, 80], [16,  800, 80],
	# #//反序两两一组依次顺时针躺下
	[29,    0, 50], [30,  600, 50], [31,    0, 80], [32, 1000, 80], [33,    0,110], [34, 1200,110], [35,    0, 150], [36,    0, 150], [37,2500,150], 
	# #//反序逆时针起来且执行速度越来越快 
	[28, 1000,100], [27,  960, 96], [26,  920, 92], [25,  880, 88], [24,  840, 84], [23,  800, 80], [22,  720, 72], [21,  600, 60], [20, 2500, 50],
	# # #//摇晃速度不变 幅度越来越大
	[19,   60,200], [18,  120,200], [19,  140,200], [18,  140,200], [19,  160,200], [18,  160,200], [19,  180,200], [18,  180,200], [19,  200,200], 
	[18,  200,200], [19,  220,200], [18,  220,200], [19,  220,200], [18,  240,200], [19,  260,200], [18,  260,200], [19,  280,200], 
	[18,  300,200], [19,  300,200], [18,  320,200], [19,  340,200], [18,  340,200], [19,  360,200], [18,  360,200], [19,  360,200], 
	[18,  400,200], [19,  440,200], [18,  500,200], [19,  500,200], [18,  560,200], [19,  600,200], [18,  600,200], [19,  600,200], 
	[18,  660,200], [19,  660,200], [18,  720,200], [19,  720,200], [18,  780,200], [19,  780,200], [18,  840,200], [19,  840,200], 
	[ 0,    0,100], [ 1,    0,100], [ 2,    0,100], [ 3,    0,100], [ 4,    0,100], [ 5,    0,100], [ 6,    0,100], [ 7,    0,100], [ 8, 2000,100], 
	# # WAVE:  T/2 = 1920ms, T_90deg = 960ms, T_delta = 240ms
	[17,  360, 96], [16,  360, 96], [15,  360, 96], [14,  360, 96], [13,  360, 96], [12,  360, 96], [11,  360, 96], [10,  360, 96], [ 9,  360, 96],
	[20,  360, 96], [21,  360, 96], [22,  360, 96], [23,  360, 96], [24,  360, 96], [25,  360, 96], [26,  360, 96], [27,  360, 96], [28,  360, 96],

	[17,  280, 96], [16,  280, 96], [15,  280, 96], [14,  280, 96], [13,  280, 96], [12,  280, 96], [11,  280, 96], [10,  280, 96], [ 9,  280, 96],
	[20,  280, 96], [21,  280, 96], [22,  280, 96], [23,  280, 96], [24,  280, 96], [25,  280, 96], [26,  280, 96], [27,  280, 96], [28,  280, 96],

	[17,  200, 96], [16,  200, 96], [15,  200, 96], [14,  200, 96], [13,  200, 96], [12,  200, 96], [11,  200, 96], [10,  200, 96], [ 9,  200, 96],
	[20,  200, 96], [21,  200, 96], [22,  200, 96], [23,  200, 96], [24,  200, 96], [25,  200, 96], [26,  200, 96], [27,  200, 96], [28,  200, 96],

	[17,  120, 96], [16,  120, 96], [15,  120, 96], [14,  120, 96], [13,  120, 96], [12,  120, 96], [11,  120, 96], [10,  120, 96], [ 9,  120, 96],
	[20,  120, 96], [21,  120, 96], [22,  120, 96], [23,  120, 96], [24,  120, 96], [25,  120, 96], [26,  120, 96], [27,  120, 96], [28,  120, 96],

	[17,   80, 96], [16,   80, 96], [15,   80, 96], [14,   80, 96], [13,   80, 96], [12,   80, 96], [11,   80, 96], [10,   80, 96], [ 9,   80, 96],
	[20,   80, 96], [21,   80, 96], [22,   80, 96], [23,   80, 96], [24,   80, 96], [25,   80, 96], [26,   80, 96], [27,   80, 96], [28,   80, 96],

	[17,  120, 96], [16,  120, 96], [15,  120, 96], [14,  120, 96], [13,  120, 96], [12,  120, 96], [11,  120, 96], [10,  120, 96], [ 9,  120, 96],
	[20,  120, 96], [21,  120, 96], [22,  120, 96], [23,  120, 96], [24,  120, 96], [25,  120, 96], [26,  120, 96], [27,  120, 96], [28, 1500, 96],
	#//全部角度归零(横躺) 标准速度
	[ 0,    0,100], [ 1,    0,100], [ 2,    0,100], [ 3,    0,100], [ 4,    0,100], [ 5,    0,100], [ 6,    0,100], [ 7,    0,100], [ 8, 3000,100], 
	# #//顺时针正序挨个竖起 但不等前一个完全竖起 下一个就要开始动作 标准速度
	[17,  130,200], [16,  130,200], [15,  130,200], [14,  130,200], [13,  130,200], [12,  130,200], [11,  130,200], [10,  130,200], [9,  130,200],
];

#//舵机对象列表
#//每个列表项代表一个舵机 里面包含了描述舵机关键属性/状态的变量
theServos = [#//设定每个舵机的初始状态
	[  1,   0,  0 ], #//1#舵机
	[  2,   0,  0 ], #//2#舵机
	[  3,   0,  0 ], #//3#舵机
	[  4,   0,  0 ], #//4#舵机
	[  5,   0,  0 ], #//5#舵机
	[  6,   0,  0 ], #//6#舵机
	[  7,   0,  0 ], #//7#舵机
	[  8,   0,  0 ], #//8#舵机
	[  9,   0,  0 ], #//9#舵机
];




def ser_open():
	global SerialHdl
	if platform.system()=='Linux':
		sdev = '/dev/ttyUSB0'
	else:
		sdev   = 'COM14'      # windows
	
	bps    = 115200       # 9600,19200,38400,57600,115200
	timout = 0.008        # timeout (second) / None : waiting forever
	try:
		SerialHdl = serial.Serial(sdev, bps, bytesize=8, parity='N', stopbits=1, timeout=timout)
	except Exception as e:
		print('error, can not open [%s]: ', sdev, e)
		SerialHdl = None

	return SerialHdl

def ser():
	global SerialHdl
	return SerialHdl

def ser_close():
	global SerialHdl
	ser().close()
	SerialHdl = None

def load_animation_unit(unit):
	return {
		"mid" :  unit[0], #motion id
		"delay": unit[1], #ms
		"speed": unit[2]
	}
def load_servo_motion_lib(motion_id, srvsum):
	global servo_motion_lib
	return servo_motion_lib[motion_id][:srvsum]
def checksum(cmd):
	cksum = 0
	for i in range(2,8):
		cksum += cmd[i]
	return cksum&0xFF

def serial_write_read(send, recv_len):
	sz = len(send)
	print('['+str(datetime.datetime.now())+'] SEND: '+('%02X '*sz)%tuple(send[:sz]))
	ser().write(bytes((send)[:len(send)]))
	recv = []
	while len(recv)<recv_len:
		rbyte = ser().read() #read one byte
		if rbyte==b'':
			print('read timeout')
			break 
		recv.append(ord(rbyte))
	sz = len(recv)
	print('['+str(datetime.datetime.now())+'] RECV: '+('%02X '*sz)%tuple(recv[:sz]))
	return recv

def send_servo_motion(servo, motion, speed):
	#servo[0]#uint8_t id;     // 舵机ID 
	#servo[1]#int16_t angle;  // 舵机当前角度 单位: 度
	#servo[2]#int16_t adjust; // 角度偏移校正值

	#motion[0]#int16_t   angle;  // 角度调整信息 单位: deg // 
	#motion[1]#int8_t    type;   // ABSA or RELA
	#motion[2]#int16_t   time;   // 指定动作完成时间
	#舵机命令组包缓冲区
	cmd = [0xFA, 0xAF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xED]
	srvid = servo[0];

	if (RELA==motion[1]) and (0==motion[0]):
		return 0 #//在舵机当前角度基础上偏移0度， 即动作为空 直接返回

	print('send_servo_motion: '+str(motion))
	if RELA==motion[1]:
	 	#相对角度 先读取舵机当前角度
		cmd[2] = srvid
		cmd[3] = 0x02  #角度回读
		cmd[4] = 0x00  
		cmd[5] = 0x00 
		cmd[6] = 0x00  
		cmd[7] = 0x00  
		cmd[8] = checksum(cmd)
		recv = serial_write_read(cmd, 10) #发送10字节 接收10字节
		if len(recv)<10:
			return 1
		servo[1] = recv[7]              #保存读回的角度值
		servo[1] += motion[0]
	else:
		servo[1] = motion[0]    #直接指定角度值
	#发送命令 调整角度
	if servo[1]<0:
		servo[1] = 0
	cmd[2] = srvid
	cmd[3] = 0x01                               #设置角度
	cmd[4] = servo[1]&0xFF                      #最终的目标角度
	cmd[5] = int(motion[2]*speed/(100*20))&0xFF #运动时间
	cmd[6] = 0x00
	cmd[7] = 0x00  
	cmd[8] = checksum(cmd)
	serial_write_read(cmd, 1)                   #发送10字节 接收1字节

	return 0

def animation_start( mode, srvlst, srvsum, animate, animate_len ):
	print('animation_start')
	step = 0       
	stop = False
	while not stop:
		if mode==DEMO_MODE_ONCE:
			stop = True
		while step<animate_len:
			au     = load_animation_unit(animate[step])
			srvmts = load_servo_motion_lib(au["mid"], srvsum)
			for i in range(0, srvsum):
				if send_servo_motion(srvlst[i], srvmts[i], au["speed"])!=0:
					return 1; #返回非0值，表示命令发送出错
			time.sleep(au["delay"]/1000)
			step += 1             


def setup():
	if ser_open()==None:
		exit(0)
	else:
		print('open success')
def loop():
	animation_start(DEMO_MODE, theServos, SERVO_SUM, servo_demo, len(servo_demo))

def main():
	setup();
	while True:
		loop()
		if DEMO_MODE==DEMO_MODE_ONCE:
			break
	ser_close()
	return

main()